#include "SINS.h"
#include "LooseCoupling.h"

CEarth::CEarth(const CVec3 &pos)
{
    Re = 6378137.0;
    f = 0.0033528106647474805;
    e2 = 0.0066943799901413156;
    wie = 7.2921151467e-5;
    g0 = 9.7803267715,
    G0 = 9.80665;
    this->init(pos);
}

void CEarth::init(const CVec3 &pos, int Method, const CVec3 &vn)
{
    this->clear();
    this->update(pos);
}

/*
 * pos：lat lon hgt (rad rad m);
 * Method ==  0：使用北纬45°附近的重力信息；
 * Method ==  1：根据纬度和高度简单计算重力信息；
 * Method >=  2: 计算相对高精度的重力信息，如地球自转；
 * Method == -1或其它：使用上一次计算的重力信息。
 */
void CEarth::update(const CVec3 &pos, int Method, const CVec3 &vn)
{
    if (fabs(pos(0)) >= math::MAX_LAT || fabs(pos(1)) >= math::MAX_LON || pos(2) >= math::MAX_HGT)
    {
        return;
    }

    if (Method >= 0)
    {
        double sl = sin(pos(0)), sl2 = sl * sl, cl = cos(pos(0));
        double sq = 1 - e2 * sl * sl, sq2 = sqrt(sq);
        RMh = Re * (1 - e2) / sq / sq2 + pos(2), f_RMh = 1.0 / RMh;
        RNh = Re / sq2 + pos(2), clRNh = cl * RNh, f_clRNh = 1.0 / clRNh;

        if (Method < 2)
        {
            if (Method == 1)
            {
                gn(2) = -(g0 * (1 + 5.27094e-3 * sl2) - 3.086e-6 * pos(2));
            }
            else // if (Method == 0)
            {
                gn(2) = -G0;
            }
            gcc = gn;
            Mpvv(0) = f_RMh, Mpvv(1) = f_clRNh;
            Mpv(0, 1) = f_RMh, Mpv(1, 0) = f_clRNh;
        }
        else
        {
            /* to do：Calculate high-precision gravity information */
        }
    }
    else
    {
        /* do nothing */
        return ;
    }
}

CVec3 CEarth::vn2dpos(const CVec3 &vn, double dt) const
{
    return CVec3(vn(1) * f_RMh, vn(0) * f_clRNh, vn(2)) * dt;
}

void CEarth::clear()
{
    // RMh = RNh = clRNh = f_RMh = f_clRNh = 0;
    wnie.setZero(), wnen.setZero(), wnin.setZero(), wnien.setZero();
    gn.setZero(), gcc.setZero(), gn(2) = -G0, gcc(2) = -G0;
    Mpvv.setZero(), Mpvv(2) = 1;
    Mpv.setZero(), Mpv(2,2) = 1;
}

CSINS::CSINS(const CVec3 &att, const CVec3 &vn, const CVec3 &pos)
{
    this->init(att, vn, pos, 0);
}

void CSINS::init(const CVec3 &att, const CVec3 &vn, const CVec3 &pos, double tk)
{
    this->clear();
    this->tk = tk, this->tk_1 = tk, this->dt = 0;
    this->att = att, this->vn = vn, this->pos = pos;
    this->Cnb = math::a2mat(att), this->qnb = math::a2qua(att);
    this->eth.init(pos);
}

/*
 * gyr：deg/s；acc：m/ss；dt：s
 */
void CSINS::update(const double *gyr, const double *acc, double tk, int nSamples)
{
    this->tk = tk;
    dt = this->tk - this->tk_1;
    const double Cc = 0.017453292519943 * dt;

    if (nSamples > 1)
    {
        wm(0) = *gyr * Cc, wm(1) = *(gyr + 1) * Cc, wm(2) = *(gyr + 2) * Cc;
        vm(0) = *acc * dt, vm(1) = *(acc + 1) * dt, vm(2) = *(acc + 2) * dt;
        wm1(0) = *(gyr + 3) * Cc, wm1(1) = *(gyr + 4) * Cc, wm1(2) = *(gyr + 5) * Cc;
        vm1(0) = *(acc + 3) * dt, vm1(1) = *(acc + 4) * dt, vm1(2) = *(acc + 5) * dt;

        CVec3 wmm = wm + wm1;
        coningErr = wm.cross(wm1) * coeff;
        phim = wmm + coningErr;

        CVec3 vmm = vm + vm1;
        rotationErr = (wmm.cross(vmm)) * 0.5;
        scullingErr = (wm.cross(vm1) + vm.cross(wm1)) * coeff;
        dvbm = vmm + scullingErr + rotationErr;
    }
    else
    {
        wm(0) = *gyr * Cc, wm(1) = *(gyr + 1) * Cc, wm(2) = *(gyr + 2) * Cc;
        vm(0) = *acc * dt, vm(1) = *(acc + 1) * dt, vm(2) = *(acc + 2) * dt;
        phim = wm, dvbm = vm;
    }

    phim -= eb * dt, dvbm -= db * dt; // imu calibration
    wbib = phim * (1.0 / dt), fb = dvbm * (1.0 / dt);
    wbeb = wbib, fn = qnb * fb;

    eth.update(pos, 1);

    an = fn + eth.gcc;
    CVec3 vn1 = vn + an * dt;
    pos = pos + eth.vn2dpos(vn + vn1, dt * 0.5), vn = vn1;
    qnb = qnb * math::rv2q(phim);
    qnb.normalize();

    Cnb = math::q2mat(qnb);
    att = math::q2att(qnb);

    this->tk_1 = this->tk;
}

void CSINS::outerLever(const CVec3 &lvr)
{
    vnL = vn + Cnb * wbeb.cross(lvr);
    posL = pos + eth.Mpv * Cnb * lvr;
}

void CSINS::clear()
{
    coeff = 2.0/ 3.0;
    tk = 0, tk_1 = 0, dt = 0;
    wm.setZero(), vm.setZero(), wm1.setZero(), vm1.setZero(), phim.setZero(), dvbm.setZero();
    coningErr.setZero(), rotationErr.setZero(), scullingErr.setZero();
    
    wbib.setZero(), wbeb.setZero(), fn.setZero(), fb.setZero(), an.setZero();
    att.setZero(), vn.setZero(), pos.setZero(), eb.setZero(), db.setZero();
    vnL.setZero(), posL.setZero();
    Cnb.setIdentity();
    qnb.setIdentity();
    // eth.clear();
}


CAvptRingBuffer::CAvptRingBuffer(void)
{
    this->clear();
}

void CAvptRingBuffer::push(double tk, const CVec3 &att, const CVec3 &vn, const CVec3 &pos)
{
    this->tk[head] = tk;
    this->att[head] = att, this->vel[head] = vn, this->pos[head] = pos;

    head++;
    head = (head < size) ? head : 0;
    cnt += (cnt < size) ? 1 : 0;
}

bool CAvptRingBuffer::interp(double tpast)
{
    if (cnt < 2) { return false; }

    int idx1, idx2;
    bool isFound = false;

    int cur = head;
    for (int k = 0; k < cnt; k++)
    {
        cur = cur - 1;
        cur = cur < 0 ? size-1 : cur;

        int last = cur - 1;
        last = last < 0 ? size-1 : last;

        if (tk[last] <= tpast && tpast <= tk[cur])
        {
            idx1 = last, idx2 = cur; 
            isFound = true;
            break;
        }
    }

    if (isFound == true)
    {
        double dt1 = tpast - tk[idx1];
        double dt2 = tk[idx2] - tpast;
        if (dt1 <= 5e-4)
        {
            atti = att[idx1], vni = vel[idx1], posi = pos[idx1];
        }
        else if (dt2 <= 5e-4)
        {
            atti = att[idx2], vni = vel[idx2], posi = pos[idx2];
        }
        else
        {
            double b = dt1 / (tk[idx2] - tk[idx1]), a = 1 - b;
            atti = a * att[idx1] + b * att[idx2], vni  = a * vel[idx1] + b * vel[idx2], posi = a * pos[idx1] + b * pos[idx2];
        }
        return true;
    }
    else
    {
        return false;
    }
}

void CAvptRingBuffer::clear()
{
    cnt = 0;
    head = 0;
    size = AVPT_RING_BUFFER_LENGTH;
    memset((void*)&tk, 0, sizeof(double)*AVPT_RING_BUFFER_LENGTH);
    for (int i = 0; i < size; i++)
    {
        att[i].setZero(), vel[i].setZero(), pos[i].setZero();
    }
    atti.setZero(), vni.setZero(), posi.setZero();
}